/*
  R BT Tank Explorer v2.0
  (c) 2011, RaJa
 */
#include <AFMotor.h>  //Подключаем заголовочный файл библиотеки AFMotor
#include <Servo.h>

byte lDirection,rDirection, Direction, lastlDir, lastrDir;
byte Header, lSpeed, rSpeed, lastlSpeed, lastrSpeed;	//Packet header, left&right motor speeds
byte pan, tilt, lastPan, lastTilt;						// Camera platform position

AF_DCMotor rMotor(4);  //Right motor
AF_DCMotor lMotor(3);  //Left motor
Servo panServo, tiltServo;
long previousMillis = 0;   
unsigned long currentMillis;
long interval = 15;    // interval at which to control servo

void setup() {
  Serial.begin(115200);
  pinMode(13, OUTPUT);     
  panServo.attach(10);	//Pan servo на D10
  tiltServo.attach(9);	//Tilt servo на D9
  Header=0;
  lSpeed=0;
  rSpeed=0;
}

void loop() {
  
  if (Serial.available()>0) {
    Header=Serial.read();
    //If header found then get and process Cmd
    if (Header==255){
      while(Serial.available()<5){};

      Direction=Serial.read();
      lSpeed=Serial.read();
      rSpeed=Serial.read();
      pan=Serial.read();
      tilt=Serial.read();
      
      lDirection=Direction & 0x03;
      rDirection=(Direction & 0x0C) >> 2;
      //Left
     if ((lastlDir!=lDirection) or (lastlSpeed!=lSpeed)){
      switch (lDirection){
        case 0:
          lMotor.run(RELEASE);
          break;
        case 1:
          lMotor.run(FORWARD);
          lMotor.setSpeed(lSpeed);
          break;
        case 2:
          lMotor.run(BACKWARD);
          lMotor.setSpeed(lSpeed);
          break;
      }
      lastlDir=lDirection;
      lastlSpeed=lSpeed;
     }

      //Right
     if ((lastrDir!=rDirection) or (lastrSpeed!=rSpeed)){
      switch (rDirection){
        case 0:
          rMotor.run(RELEASE);
          break;
        case 1:
          rMotor.run(FORWARD);
          rMotor.setSpeed(rSpeed);
          break;
        case 2:
          rMotor.run(BACKWARD);
          rMotor.setSpeed(rSpeed);
          break;
      }
      lastrDir=rDirection;
      lastrSpeed=rSpeed;
     }
      
      //Rotate camera
      currentMillis = millis();
      if(currentMillis - previousMillis > interval) {
        previousMillis = currentMillis; 
        if (lastPan!=pan) panServo.write(pan);			// tell pan servo to go to position
        if (lastTilt!=tilt) tiltServo.write(tilt);		// tell tilt servo to go to position
        lastPan=pan;
        lastTilt=tilt;
      }

    }
  }
}
